#include "IFUException.h"
#include "ImageSlice.h"
#include "Mask.h"
#include "Sifus.h"
#include "NonLinearFit.h"

#include <iostream>

#include <TObject.h>
#include <TMath.h>
#include <TMinuit.h>

using namespace std;

TMinuit *pMinuit = 0;

void fcn2(int &npar, double *gin, double &f, double *par, int iflag);
double func2(int npar, float xx, double *par);

NonLinearFit::NonLinearFit(Mask *mascara, Sifus *sifus): TObject() {

  pMinuit = new TMinuit(11);
  pMinuit->SetObjectFit(this);
  pMinuit->SetFCN(fcn2);
  pMinuit->SetPrintLevel(-2);

  this->mascara = mascara;
  this->sifus = sifus;

}

NonLinearFit::~NonLinearFit() {
  delete pMinuit;
}


void NonLinearFit::fit(ImageSlice *coluna) {

  this->coluna = coluna;
  this->yyy = new float[coluna->size()];
  for (int i = 0; i < coluna->size(); i++)
    this->yyy[i] = i;

  double parameter, erro;
  double *x0, *sigma, *amp;

  int nPeaks = mascara->par->getNPeaksPerCol();
  int currCol = coluna->getIndex();

  // ajusta os vetores
  if (currCol == 0) {
    x0 = mascara->par->getX0(currCol);
    sigma = mascara->par->getSigma(currCol);
    amp = mascara->par->getAmp(currCol);

    for (int i = 0; i < nPeaks; i++)
      amp[i] = mascara->fits->getPixel((int)x0[i], 0);

  } else {
    x0 = mascara->par->getX0(currCol -1 );
    sigma = mascara->par->getSigma(currCol -1 );
    amp = mascara->par->getAmp(currCol -1 );
  }

  for (int i=0 ; i< nPeaks ; i++) {

    mascara->setCurrPeak(i);

    // (contador, nome, valor_inicial, incerteza/passo, limite_abaixo, limite_acima)
    pMinuit->DefineParameter(0, "X0", x0[i], 0.1, x0[i] - 2.0, x0[i] + 2.0);
    pMinuit->DefineParameter(1, "SIGMA", sigma[i], 0.1/5.0, 0.0, 30.0);
    pMinuit->DefineParameter(2, "AMP", amp[i], 0.1*100.0, 0.0, 0.0);

    // executa o ajuste para os parametros dados
    pMinuit->Migrad();

    pMinuit->GetParameter(0,parameter,erro);
    mascara->par->setX0(currCol, i, parameter);

    pMinuit->GetParameter(1,parameter,erro);
    mascara->par->setSigma(currCol, i, parameter);

    pMinuit->GetParameter(2,parameter,erro);
    mascara->par->setAmp(currCol, i, parameter);

  }

  delete []x0;
  delete []sigma;
  delete []amp;
  delete []yyy;
}


double func2(int npar, float x, double *par) {

  // npar is the number of Gaussians * 3 (3 pars / Gaussian)
  // x is the independent variable
  // *par is a pointer to the parameters array
  // Gauss = Amp * exp(-0.5 * arg)
  // arg = (x-x0) * (x-x0) / 2.0 / sigma
  // par[n]   = POSITION - fixed here
  // par[n+1] = SIGMA - fixed here
  // par[n+2] = AMP

  double value;
  int n ;
  double arg1 ;

  value = 0.0 ;
  // loops around the Gaussians. Note n+=3, 3 pars / Gaussian
  for(n=0 ; n<npar ; n+=3)
    {
      arg1 = (x - par[n]) ;
      arg1 *= arg1 ;
      arg1 /= 2.0 * par[n+1] ;
      value += par[n+2] * TMath::Exp(-arg1) ;
    }

  //cout << "func2: " << value << endl;
  //cout.flush();
  return value;

}

void fcn2(int &npar, double *gin, double &f, double *par, int iflag) {

  NonLinearFit *fit = (NonLinearFit *)pMinuit->GetObjectFit();

  double chisq = 0.0;
  double delta = 0.0;

  int peak = fit->mascara->getCurrPeak();
  int start, end;
  int start_peak_pos, end_peak_pos;
  int currCol = fit->mascara->getCurrCol();

  start = end = peak;

  if ( end >= fit->mascara->par->getNPeaksPerCol()) {
    end =  fit->mascara->par->getNPeaksPerCol() - 1;
    start = end;
  }

  if (currCol == 0) {
    start_peak_pos = (int)floor(fit->mascara->par->getX0(currCol, start) - (2 * 1.3));
    end_peak_pos = (int)ceil(fit->mascara->par->getX0(currCol, end) + (2 * 1.3));
  } else {
    start_peak_pos = (int)floor(fit->mascara->par->getX0(currCol-1, start) - (2 * 1.3));
    end_peak_pos = (int)ceil(fit->mascara->par->getX0(currCol-1, end) + (2 * 1.3));
  }

  start_peak_pos++;

  //cout << start << ":" << end << " " << start_peak_pos << ":" << end_peak_pos << endl;
  
  for(int i = start_peak_pos; i < end_peak_pos; i++) {
    delta  = fit->coluna->get(i) - func2(npar, fit->yyy[i], par);
    chisq += delta * delta;
    //cout << "fcn2 " << i << " " << delta << " " << chisq << " " << fit->coluna->get(i) << " "
    //<< fit->yyy[i] << " " << func2(npar,fit->yyy[i],par) << endl;
    //cout.flush();
  }

  //cout << endl;
  //cout.flush();

  f = chisq;

}

